Teensy micro ros
Table of Content
Teensy micro ros#

Add udev rule#
copy udev file from https://www.pjrc.com/teensy/00-teensy.rules to /etc/udev/rules.d/00-teensy.rules
PlatformIO#
Install PlatformIO
[env:teensy41]
platform = teensy
board = teensy41
framework = arduino
board_microros_distro = humble
board_microros_transport = serial
monitor_speed = 115200
lib_deps =
https://github.com/micro-ROS/micro_ros_platformio
publisher example code
#include <Arduino.h>
#include <micro_ros_platformio.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <std_msgs/msg/int32.h>
#if !defined(MICRO_ROS_TRANSPORT_ARDUINO_SERIAL)
#error This example is only avaliable for Arduino framework with serial transport.
#endif
rcl_publisher_t publisher;
std_msgs__msg__Int32 msg;
rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t timer;
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}
// Error handle loop
void error_loop() {
while(1) {
delay(100);
}
}
void timer_callback(rcl_timer_t * timer, int64_t last_call_time) {
RCLC_UNUSED(last_call_time);
if (timer != NULL) {
RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
msg.data++;
}
}
void setup() {
// Configure serial transport
Serial.begin(115200);
set_microros_serial_transports(Serial);
delay(2000);
allocator = rcl_get_default_allocator();
//create init_options
RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
// create node
RCCHECK(rclc_node_init_default(&node, "micro_ros_platformio_node", "", &support));
// create publisher
RCCHECK(rclc_publisher_init_default(
&publisher,
&node,
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
"micro_ros_platformio_node_publisher"));
// create timer,
const unsigned int timer_timeout = 1000;
RCCHECK(rclc_timer_init_default(
&timer,
&support,
RCL_MS_TO_NS(timer_timeout),
timer_callback));
// create executor
RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
RCCHECK(rclc_executor_add_timer(&executor, &timer));
msg.data = 0;
}
void loop() {
delay(100);
RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
}
Agent#
Build#
ros2 run micro_ros_setup create_agent_ws.sh
ros2 run micro_ros_setup build_agent.sh
source install/local_setup.bash
Run#
ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyACM0
ROS#
ros2 topic list
/clock
/micro_ros_platformio_node_publisher
/rosout
ros2 topic echo /micro_ros_platformio_node_publisher
#
data: 159
---
data: 160